#ifndef SCAN_FILTER_HPP
#define SCAN_FILTER_HPP

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/message_filter.h>
#include <tinyxml2.h>

#include <memory>
#include <mutex>
#include <vector>
#include <cmath>
#include <limits>
#include <angles/angles.h>

namespace AgvObstaAvoid
{

class RadiusOutlierWindowValidator
{
public:
    bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_distance)
    {
        int num_neighbors = 0;
        const float& r1 = scan.ranges[idx];
        float r2 = 0.;

        for (int y = -(int)window; y < (int)window + 1 && num_neighbors < (int)window; y++) {
            int j = idx + y;
            r2 = scan.ranges[j];

            if (j < 0 || j >= static_cast<int>(scan.ranges.size()) || idx == j || std::isnan(r2)) {
                continue;
            }

            const float d = sqrt(
                pow(r1, 2) + pow(r2, 2) -
                (2 * r1 * r2 * cosf(y * scan.angle_increment)));

            if (d <= max_distance) {
                num_neighbors++;
            }
        }

        return num_neighbors >= window;
    }
};

class ScanFilterChain
{
public:
    ScanFilterChain();
    ~ScanFilterChain() = default;

    bool FilterReadXml(tinyxml2::XMLElement* element);
    void LidarSubCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg, int equip_name);
    
    bool filter_enable;
    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_filter_1_;
    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_filter_2_;
private:
    bool ShadowsFilter(const sensor_msgs::msg::LaserScan& scan_in, sensor_msgs::msg::LaserScan& scan_out);
    void ShadowsPrepareForInput(const float angle_increment);
    bool ShadowsisShadow(float r1, float r2, float included_angle_sin, float included_angle_cos);

    std::mutex own_mutex_;
    RadiusOutlierWindowValidator validator_;
    std::vector<bool> valid_ranges_work_;

    // Filter parameters
    int filter_type_;
    double speckle_max_range_;
    double max_range_difference_;
    int filter_window_;

    double min_angle_shadows_;
    double max_angle_shadows_;
    double window_shadows_;
    int neighbors_shadows_;
    bool remove_shadow_start_point_shadows_;

    std::vector<float> sin_map_shadows_;
    std::vector<float> cos_map_shadows_;
    float angle_increment_shadows_;
    float min_angle_tan_, max_angle_tan_;

    bool speckleFilter_enable_;
    bool shadowsFilter_enable_;
};

} // namespace AgvObstaAvoid

#endif // SCAN_FILTER_HPP